%% 四架无人机
clc; clear; 
close all;
format long;

%% parameters
global dt Quadrotor_Num
global Tf
parameters;
t = 0 : dt : Tf;
N = Tf/dt + 1;
% UAV's flag
flag = zeros(Quadrotor_Num,1); 
for Num = 1:Quadrotor_Num
    flag(Num) = Num;
end
%% Initialization Position1
x0 = zeros(12,Quadrotor_Num + 1);
% 无人机状态
x0(:,1) = [0;   0;  0; 0; 0; 0;
           1.5; 0;  5.2; 0; -1.1; 0];   % Follower1
x0(:,2) = [0;   0;  0; 0; 0; 0;
           1.5; 0;  4.4; 0; -1.1; 0];   % Follower2
x0(:,3) = [0;   0;  0; 0; 0; 0;
          0.5;  0;  5.2; 0; -1.1; 0];   % Follower3
x0(:,4) = [0;   0;  0; 0; 0; 0;
          0.5;  0;  4.4; 0; -1.1; 0];   % Follower4
% 物体状态
x0(:,5) = [0; 0; 0; 0; 0; 0;
           1; 0; 4.8; 0; 0; 0]; % Object 几何位置
x = x0;
x_New = zeros(13,Quadrotor_Num + 1);
% 标记UAV
x_New(1,:) = 1;
x_New(2:13,:) = x;

% rho 物体质心指向悬挂边缘的点
rho = [0.5  0.5 -0.5 -0.5;
       0.4 -0.4  0.4 -0.4;
      -0.1 -0.1 -0.1 -0.1];

% q_i 无人机质心指向连接平台位置初始值(初始值为[0,0,1]'，这里写q_i为0，是用来计算q_i_dot)
qi = zeros(3,Quadrotor_Num);

% 无人机质心指向连接平台位置预期位置解算值初始值
qi_d = zeros(3,Quadrotor_Num);

% 绳索角速度初始值
sOmega = zeros(3,Quadrotor_Num);

% 保存输出画图数据
X_UAV_save = zeros(N,12, Quadrotor_Num);    % States
X_L_save = zeros(N,12);

e_3 = [0; 0; 1];

pL_d = zeros(3,N);
pL_d_dot = zeros(3, N);

RL_d = zeros(3,3, N); % 

Att_L_d = zeros(3,N); % phi、 theta、 psi
Ome_L_d = zeros(3,N); % phi、 theta、 psi 导数

% 四阶龙格库塔
k1_plant = zeros(12,Quadrotor_Num); 
k2_plant = zeros(12,Quadrotor_Num);
k3_plant = zeros(12,Quadrotor_Num); 
k4_plant = zeros(12,Quadrotor_Num);

% 记录无人机姿态角速度的预期值，要经过迭代
omegaid_p = zeros(3,Quadrotor_Num);
sOmega_d = zeros(3,Quadrotor_Num);
Att_L_d_Last = zeros(3,1);

Ome_L = zeros(3,1);

% UAV控制输入保存
F_plant_Save = zeros(N + 1,Quadrotor_Num);
F_plant_Save(1,:) = zeros(1,4);

M_plant_Save = zeros(N + 1,3,Quadrotor_Num);
M_plant_Save(1,:,:) = zeros(3,Quadrotor_Num);

% 货物控制输入保存
F_in_Save = zeros(N,3);
M_in_Save = zeros(N,3);
qi_Save = zeros(N,3,4);

for i = 1:N
    % 货物运输轨迹Desire of Position
    omega_d = 2 * pi / 40;
    xl_d = 1.5 * sin(omega_d * t(i));
    yl_d = 4.2 * cos(omega_d * t(i) / 2);
    zl_d = -0.5; % e = x - xd  PID = k_p * e + k_d * e_dot + k_i * e_i
    pL_d(:,i) = [xl_d yl_d zl_d]';
    
    xl_d_dot = 1.5 * omega_d * cos(omega_d * t(i));
    yl_d_dot = -4.2 * 0.5 * omega_d * sin(omega_d * t(i) / 2);
    zl_d_dot = 0;
    pL_d_dot(:,i) = [xl_d_dot yl_d_dot zl_d_dot]';

    % 预期姿态生成
    if norm(pL_d_dot(:,i)) < 1e-3
        RL_d(:,:,i) = eye(3); % 零速度时保持水平
    else
        Rl_d1 = pL_d_dot(:,i) / (norm(pL_d_dot(:,i)) + 1e-6);
        Rl_d3 = e_3;
        Rl_d2 = cross(Rl_d3, Rl_d1);
        Rl_d2 = Rl_d2 / norm(Rl_d2);
        RL_d(:,:,i) = [Rl_d1, Rl_d2, Rl_d3];
    end
    % 欧拉角（X-Y-Z顺序）
    angles_eul = rotm2eul(RL_d(:,:,i), 'XYZ');
    Att_L_d(:,i) = angles_eul'; % [φ; θ; ψ]

    % 角速度（Body坐标系）
    if i > 1
        R_prev = RL_d(:,:,i - 1);
        R_current = RL_d(:,:,i);
        R_dot = (R_current - R_prev) / dt;
        omega_skew = R_current' * R_dot;
        Ome_L_d(:,i) = [omega_skew(3,2); omega_skew(1,3); omega_skew(2,1)];
    else
        Ome_L_d(:,i) = zeros(3,1);
    end
    
    [F_in, M_in, F_plant, M_plant, qi_d, qi, a_all, U_s, sOmega, sOmega_d, omegaid_p, Ome_L] = controller_o_i(x_New, qi_d, sOmega, sOmega_d, qi, Att_L_d(:,i), Ome_L, Ome_L_d(:,i), pL_d(:,i), pL_d_dot(:,i), omegaid_p, rho, RL_d(:,:,i));

    % 保存控制输入
    F_plant_Save(i + 1,:) = F_plant;
    M_plant_Save(i + 1,:,:) = M_plant;

    F_in_Save(i, :) = F_in;
    M_in_Save(i, :) = M_in;

    qi_Save(i,:,:) = qi;

    % 传入到货物中
    k1_o = plant_load(x_New(2:13,5),             F_in, M_in);
    k2_o = plant_load(x_New(2:13,5) + dt/2*k1_o, F_in, M_in);
    k3_o = plant_load(x_New(2:13,5) + dt/2*k2_o, F_in, M_in);
    k4_o = plant_load(x_New(2:13,5) + dt * k3_o, F_in, M_in);
    x_New(2:13,5) = x_New(2:13,5) + dt/6 *(k1_o + 2*k2_o + 2*k3_o + k4_o);
    x_o = [x_New(8,Quadrotor_Num + 1); x_New(10,Quadrotor_Num + 1); x_New(12,Quadrotor_Num + 1)];

    % 当前货物的旋转矩阵
    x_Att_L = [x_New(2, Quadrotor_Num + 1); x_New(4, Quadrotor_Num + 1); x_New(6, Quadrotor_Num + 1)];
    RL_cur = eul2rotm_ZYX(x_Att_L);

    for q_num = 1 : Quadrotor_Num
        % x_New(8, q_num) = x_o + RL_cur*rho - l_plant(q_num) * qi(:,q_num);
        % 先计算右侧向量（假设结果为3×1）
        x_plant = x_o + RL_cur*rho - 1 * qi(:,q_num);
        
        % 将三个分量分别写入指定位置
        x_New(8,  q_num) = x_plant(1);  % 第1个分量写入(8, q_num)
        x_New(10, q_num) = x_plant(2);  % 第2个分量写入(10, q_num)
        x_New(12, q_num) = x_plant(3);  % 第3个分量写入(12, q_num)
    end


%     % 传入到无人机和绳索中
%     for plant_i = 1 : Quadrotor_Num
%         % 无人机
%         k1_plant(:, plant_i) = plant_qua(x_New(2:13,plant_i),                               F_plant(plant_i), M_plant(:,plant_i), U_s(:, plant_i));
%         k2_plant(:, plant_i) = plant_qua(x_New(2:13,plant_i) + dt/2 * k1_plant(:, plant_i), F_plant(plant_i), M_plant(:,plant_i), U_s(:, plant_i));
%         k3_plant(:, plant_i) = plant_qua(x_New(2:13,plant_i) + dt/2 * k2_plant(:, plant_i), F_plant(plant_i), M_plant(:,plant_i), U_s(:, plant_i));
%         k4_plant(:, plant_i) = plant_qua(x_New(2:13,plant_i) + dt   * k3_plant(:, plant_i), F_plant(plant_i), M_plant(:,plant_i), U_s(:, plant_i));
%         x_New(2:13,plant_i) = x_New(2:13,plant_i) + dt/6 * (k1_plant(:, plant_i) + 2 * k2_plant(:, plant_i) + 2 * k3_plant(:, plant_i) + k4_plant(:, plant_i));
%     end
    
    for plant_i = 1 : 4
        X_UAV_save(i,:,plant_i) = x_New(2:13,plant_i);
    end
    X_L_save(i,:) = x_New(2:13,5);


end

%% 3-D Space
% steps
stepping = 500; 
% Linestyles
marker_shapes = cell(1,Quadrotor_Num);
for Num = 1:Quadrotor_Num
    shapes = {'^', 'd', 's', 'o', '+', '*', 'x'};
    marker_shapes{Num} = shapes{mod(Num - 1, length(shapes)) + 1};
end

figure
for Num = 1:Quadrotor_Num
    plot3(X_UAV_save(1:stepping:N, 7, Num), X_UAV_save(1:stepping:N, 9, Num), X_UAV_save(1:stepping:N, 11, Num), marker_shapes{Num}, 'Linewidth', 2);
    hold on;
end
% for Num = 1:Quadrotor_Num
%     plot3(X_UAV_save(1:stepping:N, 7, Num), X_UAV_save(1:stepping:N, 9, Num), X_UAV_save(1:stepping:N, 11, Num),'Linewidth', 2);
%     hold on;
% end
% pointNUM = 1:stepping:N;
% for Num = pointNUM
%      %% Four UAVS
%      for j = 1:Quadrotor_Num
%         plot3([X_UAV_save(Num,7,j),  X_UAV_save(Num,7,mod(j,Quadrotor_Num) + 1)], ...
%               [X_UAV_save(Num,9,j),  X_UAV_save(Num,9,mod(j,Quadrotor_Num) + 1)], ...
%               [X_UAV_save(Num,11,j), X_UAV_save(Num,11,mod(j,Quadrotor_Num) + 1)], 'Linewidth', 1.5);
%         hold on
%      end
% end

% 实际轨迹
figure 
plot3(X_L_save(:,7),X_L_save(:,9),X_L_save(:,11),'Color','#0000FF','Linewidth',3);
title('货物实际三维轨迹');
xlabel('X');
ylabel('Y');
zlabel('Z');hold on
grid on;


% % 实际轨迹
% figure 
% plot3(X_L_save((35000:end),7),X_L_save((35000:end),9),X_L_save((35000:end),11),'Color','#0000FF','Linewidth',3);
% title('货物实际三维轨迹');
% xlabel('X');
% ylabel('Y');
% zlabel('Z');hold on
% grid on;


% 预期的轨迹
figure 
plot3(pL_d(1,:),pL_d(2,:),pL_d(3,:),'Color','#0000FF','Linewidth',3);
title('货物预期三维轨迹');
xlabel('X');
ylabel('Y');
zlabel('Z');hold on
grid on;

% Position 
figure
quatrotor_plot(t, pL_d(1,:),'-b'); 
hold on
quatrotor_plot(t, pL_d(2,:),'-r');
quatrotor_plot(t, pL_d(3,:), '-g')
grid on;
title('货物预期轨迹二维图');
legend('X','Y','Z');


figure
% Position 
quatrotor_plot(t, X_L_save(:,7),'-b'); 
hold on
quatrotor_plot(t, X_L_save(:,9),'-r');
quatrotor_plot(t, X_L_save(:,11), '-g')
grid on;
title('货物实际轨迹二维图');
legend('X','Y','Z');

%% 绘制期望偏航角psi角
% 实际姿态
figure
quatrotor_plot(t, X_L_save(:,1),'-b'); 
hold on
quatrotor_plot(t, X_L_save(:,3),'-r');
quatrotor_plot(t, X_L_save(:,5), '-g')
grid on;
title('货物实际姿态');
legend('实际滚转角\phi','实际俯仰角\theta','实际偏航角\psi');

% 预期姿态
figure
quatrotor_plot(t, Att_L_d(1,:),'-b'); 
hold on
quatrotor_plot(t, Att_L_d(2,:),'-r');
quatrotor_plot(t, Att_L_d(3,:), '-g')
grid on;
title('货物预期姿态');
legend('预期滚转角\phi','预期俯仰角\theta','预期偏航角\psi');

figure
% Position 
quatrotor_plot(t, X_UAV_save(:,7,1),'-b'); 
hold on
quatrotor_plot(t, X_UAV_save(:,9,1),'-r');
quatrotor_plot(t, X_UAV_save(:,11,1), '-g')
grid on;
title('无人机实际轨迹二维图');
legend('X','Y','Z');


%% 
function v = vee(S)
    % S 为 3x3 斜对称矩阵, 返回对应的 3x1 向量
    v = [S(3,2); S(1,3); S(2,1)];
end
%% ================== 斜对称矩阵生成函数 ==================
function S = skew(v)
    % v 为 3x1 向量, S 则为 3x3 反对称矩阵,满足 S*w = v x w
    S = [   0,   -v(3),  v(2);
         v(3),     0,   -v(1);
        -v(2),   v(1),    0   ];
end

function euler_angles = rotation_matrix_to_euler(R)
    % 确保矩阵是3x3
    if size(R,1) ~= 3 || size(R,2) ~= 3
        error('输入的旋转矩阵必须是3x3矩阵');
    end
    
    % 提取旋转矩阵的元素
    R11 = R(1,1);
    R12 = R(1,2);
    R13 = R(1,3);
    R21 = R(2,1);
    R22 = R(2,2);
    R23 = R(2,3);
    R31 = R(3,1);
    R32 = R(3,2);
    R33 = R(3,3);
    
    % 计算欧拉角（ZYX顺序）
    yaw = atan2(R21, R11);  % 绕Z轴的旋转
    pitch = atan2(-R31, sqrt(R11^2 + R21^2));  % 绕Y轴的旋转
    roll = atan2(R32, R33);  % 绕X轴的旋转
    
    % 返回欧拉角
    euler_angles = [roll, pitch, yaw]';  % 顺序为 [roll, pitch, yaw]
end

function R = eul2rotm_ZYX(angles)
    % angles = [roll; pitch; yaw]，单位：弧度
    % 输出 R：3x3旋转矩阵
    %
    % 如果你的 angles 顺序或含义不同，
    % 请自行修改这部分的映射关系。

    roll  = angles(1);
    pitch = angles(2);
    yaw   = angles(3);

    % 基本旋转矩阵
    Rz = [ cos(yaw), -sin(yaw), 0;
           sin(yaw),  cos(yaw), 0;
           0,         0,        1 ];

    Ry = [ cos(pitch),  0, sin(pitch);
           0,           1, 0;
          -sin(pitch),  0, cos(pitch) ];
    
    Rx = [ 1, 0,         0;
           0, cos(roll), -sin(roll);
           0, sin(roll),  cos(roll) ];

    % 按 ZYX 顺序做右乘
    % R = Rz(yaw)*Ry(pitch)*Rx(roll)
    R = Rz * Ry * Rx;
end